#include "ingsoc.h"
#include "platform_api.h"
void motor_setup(void)
{
    SYSCTRL_ClearClkGateMulti( 0 | (1 << SYSCTRL_ITEM_APB_PinCtrl)
                                 | (1 << SYSCTRL_ITEM_APB_GPIO0)
                                 | (1 << SYSCTRL_ITEM_APB_TMR0)
                                 | (1 << SYSCTRL_ITEM_APB_TMR1));
// TIMER0 2*16 PWM
    TMR_SetOpMode(APB_TMR0, 0, TMR_CTL_OP_MODE_16BIT_PWM, 0, 0);
    TMR_SetReload(APB_TMR0, 0, (0x05DCu << 16) | 0x05DC);
    TMR_Enable   (APB_TMR0, 0, (0 << 3));
    TMR_SetOpMode(APB_TMR0, 1, TMR_CTL_OP_MODE_16BIT_PWM, 0, 0);
    TMR_SetReload(APB_TMR0, 1, (0x05DCu << 16) | 0x05DC);
    TMR_Enable   (APB_TMR0, 1, (0 << 3));


 // TIMER1 2*16 PWM
    TMR_SetOpMode(APB_TMR1, 0, TMR_CTL_OP_MODE_16BIT_PWM, 0, 0);
    TMR_SetReload(APB_TMR1, 0, (0x05DCu << 16) | 0x05DC);
    TMR_Enable   (APB_TMR1, 0, (0 << 3));
    TMR_SetOpMode(APB_TMR1, 1, TMR_CTL_OP_MODE_16BIT_PWM, 0, 0);
    TMR_SetReload(APB_TMR1, 1, (0x05DCu << 16) | 0x05DC);
    TMR_Enable   (APB_TMR1, 1, (0 << 3));

//    PINCTRL_SetPadMux(10, IO_SOURCE_TIMER1_PWM0_A);
//    PINCTRL_SetPadMux(12, IO_SOURCE_TIMER1_PWM1_A);
//    PINCTRL_SetPadMux(6, IO_SOURCE_TIMER0_PWM0_A);
//    PINCTRL_SetPadMux(8, IO_SOURCE_TIMER0_PWM1_A);
    PINCTRL_SetPadMux(6, IO_SOURCE_GPIO);
    GIO_SetDirection(6,GIO_DIR_OUTPUT);    
    GIO_WriteValue(6,0);
    
    PINCTRL_SetPadMux(8, IO_SOURCE_GPIO);
    GIO_SetDirection(8,GIO_DIR_OUTPUT);
    GIO_WriteValue(8,0); 
    
    PINCTRL_SetPadMux(10, IO_SOURCE_GPIO);
    GIO_SetDirection(10,GIO_DIR_OUTPUT);    
    GIO_WriteValue(10,0);  
 
    GIO_SetDirection(12,GIO_DIR_OUTPUT);    
    PINCTRL_SetPadMux(12, IO_SOURCE_GPIO);
    GIO_WriteValue(12,0);      
}
void timer0_pwm0_reload(uint16_t frequency, uint8_t duty)
{
    platform_printf("%s call\r\n",__FUNCTION__);
    static uint8_t last_duty = 255;
    if(last_duty == duty) return;
    last_duty = duty;
    if(duty > 0){
        PINCTRL_SetPadMux(6, IO_SOURCE_TIMER0_PWM0_A);
    }
    else{ 
        APB_TMR0->ChEn &= ~(0xf << (0 * 4));
        PINCTRL_SetPadMux(6, IO_SOURCE_GPIO);
        return;
    }
        
    uint32_t pera = SYSCTRL_GetClk(SYSCTRL_ITEM_APB_TMR0) / frequency;
    
    uint32_t reValue = (pera-pera*duty/100) | (pera*duty/100) << 16;    
    
    APB_TMR0->ChEn &= ~(0xf << (0 * 4));
    APB_TMR0->Channels[0].Reload = reValue;
    APB_TMR0->ChEn |= ((0xf & 0xf) << (0 * 4));
}

void timer0_pwm1_reload(uint16_t frequency, uint8_t duty)
{
    platform_printf("%s call\r\n",__FUNCTION__);
    static uint8_t last_duty = 255;
    if(last_duty == duty) return;
     last_duty = duty;
    if(duty > 0){
        PINCTRL_SetPadMux(8, IO_SOURCE_TIMER0_PWM1_A);
    }
    else{ 
        APB_TMR0->ChEn &= ~(0xf << (1 * 4));
        PINCTRL_SetPadMux(8, IO_SOURCE_GPIO);
        return;
    }    
    uint32_t pera = SYSCTRL_GetClk(SYSCTRL_ITEM_APB_TMR0) / frequency;
    
    uint32_t reValue = (pera-pera*duty/100) | (pera*duty/100) << 16;    
    
    APB_TMR0->ChEn &= ~(0xf << (1 * 4));
    APB_TMR0->Channels[1].Reload = reValue;
    APB_TMR0->ChEn |= ((0xf & 0xf) << (1 * 4));
}

void timer1_pwm0_reload(uint16_t frequency, uint8_t duty)
{
    platform_printf("%s call\r\n",__FUNCTION__);    
    static uint8_t last_duty = 255;
    if(last_duty == duty) return;
     last_duty = duty;
    if(duty > 0) {
        PINCTRL_SetPadMux(10, IO_SOURCE_TIMER1_PWM0_A);
    }
    else{ 
        APB_TMR1->ChEn &= ~(0xf << (0 * 4));
        PINCTRL_SetPadMux(10, IO_SOURCE_GPIO);
        return;
    }       
    uint32_t pera = SYSCTRL_GetClk(SYSCTRL_ITEM_APB_TMR1) / frequency;
    
    uint32_t reValue = (pera-pera*duty/100) | (pera*duty/100) << 16;    
    
    APB_TMR1->ChEn &= ~(0xf << (0 * 4));
    APB_TMR1->Channels[0].Reload = reValue;
    APB_TMR1->ChEn |= ((0xf & 0xf) << (0 * 4));
}

void timer1_pwm1_reload(uint16_t frequency, uint8_t duty)
{
    platform_printf("%s call\r\n",__FUNCTION__);    
    static uint8_t last_duty = 255;
    if(last_duty == duty) return;
    last_duty = duty;
    if(duty > 0) {
        PINCTRL_SetPadMux(12, IO_SOURCE_TIMER1_PWM1_A);
    }
    else{ 
        APB_TMR1->ChEn &= ~(0xf << (1 * 4));
        PINCTRL_SetPadMux(12, IO_SOURCE_GPIO);
        return;
    }       
    uint32_t pera = SYSCTRL_GetClk(SYSCTRL_ITEM_APB_TMR1) / frequency;
    
    uint32_t reValue = (pera-pera*duty/100) | (pera*duty/100) << 16;
    
    APB_TMR1->ChEn &= ~(0xf << (1 * 4));
    APB_TMR1->Channels[1].Reload = reValue;
    APB_TMR1->ChEn |= ((0xf & 0xf) << (1 * 4));
}

void fly_ctrl(uint8_t motor_mask, uint8_t power)
{
    if(power >= 100)  power = 99;
    platform_printf("duty:%d\r\n",power);
    if(motor_mask&0x01) timer0_pwm0_reload(2000,      power);//io 6
    if(motor_mask&0x02) timer0_pwm1_reload(2000,      power);//io 8
    if(motor_mask&0x04) timer1_pwm0_reload(2000,      power);//io 10
    if(motor_mask&0x08) timer1_pwm1_reload(2000,      power);//io 12
}

